#include "stereo_camera.h"

int StereoCamera::save(YAML::Node& node) {
    node["StereoCamera"] = YAML::Node(YAML::NodeType::Map);
    node["StereoCamera"]["cameraModel"] = _cameraModel;
    node["StereoCamera"]["cameraType"] = _cameraType;
    node["StereoCamera"]["resolutionWidth"] = _resolutionWidth;
    node["StereoCamera"]["resolutionHeight"] = _resolutionHeight;
    node["StereoCamera"]["rgbFrameRate"] = _rgbFrameRate;
    node["StereoCamera"]["fovWidth"] = _fovWidth;
    node["StereoCamera"]["fovHeight"] = _fovHeight;
    node["StereoCamera"]["depthFrameRate"] = _depthFrameRate;
    return 0;
}

int StereoCamera::setCameraModel(std::string model) {
    _cameraModel = model;
    return 0;
}

StereoCamera::StereoCamera() {
    _cameraModel = "RealSense D435i";
    _cameraType = "D430";
    _resolutionWidth = 1920;
    _resolutionHeight = 1080;
    _rgbFrameRate = 30;
    _fovWidth = 87;
    _fovHeight = 58;
    _depthFrameRate = 90;
}

void StereoCamera::print() {
    cout << "StereoCamera:" << endl;
    cout << "\tcameraModel: " << _cameraModel << endl;
    cout << "\tcameraType: " << _cameraType << endl;
    cout << "\tresolutionWidth: " << _resolutionWidth << " x " << _resolutionHeight << endl;
    cout << "\trgbFrameRate: " << _rgbFrameRate << "Hz" << endl;
    cout << "\tfovWidth: " << _fovWidth << "°" << endl;
    cout << "\tfovHeight: " << _fovHeight << "°" << endl;
    cout << "\tdepthFrameRate: " << _depthFrameRate << "Hz" << endl;
}

void StereoCamera::setCameraType(std::string type) {
    _cameraType = type;
}

void StereoCamera::setResolution(int width, int height) {
    _resolutionWidth = width;
    _resolutionHeight = height;
}

void StereoCamera::setRGBFrameRate(int frameRate) {
    _rgbFrameRate = frameRate;
}

void StereoCamera::setFOV(int width, int height) {
    _fovWidth = width;
    _fovHeight = height;
}

void StereoCamera::setDepthFrameRate(int frameRate) {
    _depthFrameRate = frameRate;
}